//#include "PrincipalHeader.h"
#include "../include/PrincipalHeader.h"

using namespace cv;
using namespace std;

void featureDetection(Mat img1, Mat img2, vector<DMatch> matches,
		vector<Point2f> imgpts1,vector<Point2f> imgpts2) {

	      // detecting keypoints
		  int minHessian = 400;
		  SurfFeatureDetector detector(minHessian);
		  vector<KeyPoint> keypoints1, keypoints2;
		  detector.detect(img1, keypoints1);
		  detector.detect(img2, keypoints2);

		  // descriptors
		  SurfDescriptorExtractor extractor;
		  Mat descriptors1, descriptors2;
		  extractor.compute(img1, keypoints1, descriptors1);
		  extractor.compute(img2, keypoints2, descriptors2);

		  // matching descriptors
		  BFMatcher matcher(NORM_L2);
		  matcher.match(descriptors1, descriptors2, matches);

	  /* Trying to filter out the mistakes of the matcher by fixing a
	   * limit to the distance*/
		 int n = matches.size();
		  for(int i = 0; i<n; i++) {
			 DMatch temp = matches[i];
			 bool remove = !(temp.distance<0.15);
			 if(remove) {
				 matches.erase(matches.begin() + i);
				 i--;
			 }
		  }

		  // draw matches
		  Mat img_matches;
		  drawMatches( img1, keypoints1, img2, keypoints2, matches, img_matches);

		  imshow("Matches", img_matches);

	  ///

		  for ( unsigned int i = 0; i < matches.size(); i++)
		  {
			  imgpts1.push_back(keypoints1[matches[i].queryIdx].pt);
			  imgpts2.push_back(keypoints2[matches[i].trainIdx].pt);

		  }

}

void SFM(Mat img1, Mat img2) {

	vector<DMatch> matches;
	vector<Point2f> imgpts1, imgpts2;
	featureDetection(img1,img2,matches,imgpts1,imgpts2);


	  Mat F = findFundamentalMat( imgpts1, imgpts2, FM_RANSAC, 0.1, 0.99);
	  //Mat_<double> E = K.t() * F * K;

	  Matx34d P(1,0,0,0,
			    0,1,0,0,
			    0,0,1,0);

	  SVD svd(F);
	  Matx33d W(0,-1,0,
			  	1,0,0,
			  	0,0,1);
	  Mat_<double> R = svd.u * Mat(W) * svd.vt;
	  Mat_<double> T = svd.u.col(2);

	  Matx34d P1(R(0,0),R(0,1),R(0,2),T(0),
			  R(1,0),R(1,1),R(1,2),T(1),
			  R(2,0),R(2,1),R(2,2),T(2));



	  waitKey(0);
}

int main( int argc, char** argv )
{
	Mat img1, img2;
	img1 = imread(argv[1], CV_LOAD_IMAGE_COLOR);
	img2 = imread(argv[2], CV_LOAD_IMAGE_COLOR);

  if( argc < 2 ||  ( !img1.data && !img2.data) )
    {
      printf( "No image data \n" );
      return -1;
    }

  SFM(img1,img2);


  return 0;
}





